#include "sens_receiver.h"
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <pthread.h>

static tsens_data empty_sens_data  = {
  0,
  0,
  0,
  0,
  0.0,
  0
};

/* not thread safe, but one struct per connection, up to MAX_CON connections */
struct _con {
  int port;
  tsens_callback callback;
  int sock;
  struct sockaddr_in echoserver;
  struct sockaddr_in echoclient;
  unsigned int serverlen;
  unsigned int clientlen;
  int is_open;
};
static struct _con cons[MAX_CON];
static volatile int do_receive[MAX_CON];

/* returns the index of the connection with the 
 * specified port it it exists, -1 otherwise */
static int find_con(int port) {
  int i=0;
  while (i < MAX_CON) {
    if (cons[i].port == port) 
      return i;
    i += 1;
  }
  return -1;
}

/* returns the index of the next free connection slot.
 * Here we have a potential race condition, but unlikely.
 */
static int find_free() {
  int i=0;
  while (i < MAX_CON) {
    if (cons[i].port == 0) {
      return i;
    }
    i += 1;
  }
  return -1;
}

#define BUFLEN 1024

int parse(char* buf, tsens_data *sens_data);

int sens_open(int port, tsens_callback callback) {
  struct _con *con;
  int i = find_con(port);
  if (i == -1) {
    i = find_free();
    if (i == -1)
      return -1;
    /* ok, we have a slot */
    (cons+i)->port = port; /* mark it busy */
  }
  con = cons+i;
  /* con->port = port; not needed already done */
  con->is_open = 0;
  con->callback = callback;
  con->sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (con->sock < 0)
    return -1;
  memset(&con->echoserver, 0, sizeof(con->echoserver));  
  con->echoserver.sin_family = AF_INET;  
  con->echoserver.sin_addr.s_addr = htonl(INADDR_ANY);  
  con->echoserver.sin_port = htons(port);  
  con->serverlen = sizeof(con->echoserver);  
  if(bind(con->sock, (struct sockaddr *) &con->echoserver, con->serverlen) < 0) 
    return -1;
  con->clientlen = sizeof(con->echoclient);
  con->is_open = 1;
  return 0;
}

int sens_close(int port) {
  struct _con *con;
  int i = find_con(port);
  if (i == -1)
    return 0; /* nothing registered for that port */
  con = cons+i;
  if (!con->is_open) 
    return 0;
  /* nothing needed here (?) */
  do_receive[i] = 0;
  con->is_open = 0;
  return 0;
}

int receive_one(int port) {
  tsens_data sens_data;
  char buffer[BUFLEN];  
  int rec = 0;  
  struct _con *con;
  int i = find_con(port);

  if (i == -1) 
    return -1;
  con = cons+i;

  if (!con->is_open) 
    return -1;
  if((rec = recvfrom(con->sock, buffer, BUFLEN, 0, 
		     (struct sockaddr*) & con->echoclient, 
		     &con->clientlen)) < 0)
    return -2;
  /*
  printf("client connected: %s\nData: %s\n\n", 
	 inet_ntoa(echoclient.sin_addr),
	 buffer);  
  */
  parse(buffer, &sens_data);
  return con->callback(sens_data);
}


int is_digit(char c) {
  return ('0' <= c) && (c <= '9');
}

double atoi_for_locale_independant(char *s, char** next) {
  double ret=0.0;
  int sign = 1;

  if (*s == 0)
    return ret;
  else if (*s == '+') {
    s++;
  } else if (*s == '-') {
    sign = -1;
    s++;
  }
  while (is_digit(*s)) {
    ret *= 10.0;
    ret += (*s - '0');
    s++;
  }
  *next = s;
  return ret*sign;
}

/* Construct a double from a string not aknowleding locales.
 * Horrible, why is there none in the standard library.
 */
double atof_locale_independant(char *s) {
  char *next = s;
  int sign = 1;
  double h = 0.0;
  double ret = 0.0;
  /*
  double test = atof(s);
  double eps = 0.00001;
  */

  if (*s == '+') {
    s++;
  } else if (*s == '-') {
    sign = -1;
    s++;
  }
  h = atoi_for_locale_independant(s, &next);
  ret = (double) h;
  if (*next == '.') {
    double divider = 1.0;
    int i;
    s = next+1;
    h = atoi_for_locale_independant(s, &next);    
    for (i=0; i < next-s; i++)
      divider *= 10.0;
    h /= ((double) divider);
    ret += h;
  }
  if ((*next == 'e') || (*next == 'E')) {
    double factor = 1;
    s = next+1;
    h = atoi_for_locale_independant(s, &next);
    if (h < 0) {
      factor = -1;
      h = -h;
    }
    while (h--) {
      factor *= 10.0;
    }
    if (factor < 0) {
      ret /= (-factor);
    } else {
      ret *= factor;
    }
  }
  ret *= sign;
  /*
  if ((ret < (test-eps)) || ((test -eps)  > ret)) {
    printf("double = %f\n", ret); 
    printf("test   = %f\n", test); 
  }
  */
  return ret;
}

static int next_devid = 0;
static char devices[20][20]; /* at most 20 devices with 20 char each */

int get_devid(char* valuebuf) {
  int i=0;

  valuebuf[19] = 0; /* if too long cut */
  while (i < next_devid) {
    if (strcmp(valuebuf, devices[i]) == 0) {
      return i+1;
    }
    i += 1;
  }
  /* not yet in there */
  strcpy(devices[next_devid++], valuebuf);
  return next_devid;
}

int handle_key_value(char* keybuf, char* valuebuf, tsens_data *sens_data) {
  /* printf("%s: %s\n", keybuf, valuebuf); */
  if (strcmp(keybuf, "x") == 0) {
    sens_data->x = atof_locale_independant(valuebuf);
  } else if (strcmp(keybuf, "y") == 0) {
    sens_data->y = atof_locale_independant(valuebuf);
  } else if (strcmp(keybuf, "z") == 0) {
    sens_data->z = atof_locale_independant(valuebuf);
  } else if (strcmp(keybuf, "timestamp") == 0) {
    sens_data->timestamp = atof_locale_independant(valuebuf);
  } else if (strcmp(keybuf, "k") == 0) {
    unsigned int k = 0;
    while (*valuebuf) {
      switch (*valuebuf) {
      case 'u' : k |= KEY_UP; break;
      case 'd' : k |= KEY_DOWN; break;
      case 'l' : k |= KEY_LEFT; break;
      case 'r' : k |= KEY_RIGHT; break;
      case 'f' : k |= KEY_FIRE; break;
      case '0' : k |= KEY_0; break;
      case '1' : k |= KEY_1; break;
      case '2' : k |= KEY_2; break;
      case '3' : k |= KEY_3; break;
      case '4' : k |= KEY_4; break;
      case '5' : k |= KEY_5; break;
      case '6' : k |= KEY_6; break;
      case '7' : k |= KEY_7; break;
      case '8' : k |= KEY_8; break;
      case '9' : k |= KEY_9; break;
      case '*' : k |= KEY_STAR; break;
      case '#' : k |= KEY_HASH; break;
      /* else ignore, shouldn't happen */
      }
      valuebuf++;
    } 
    sens_data->keys = k;
  } else if (strcmp(keybuf, "devid") == 0) {
    sens_data->devid = get_devid(valuebuf);
  } else {
    /* not handled, ignored */
    return 1;
  }
  return 0;
}

int is_in(char c, char* to_test) {
  while (*to_test) {
    if (*to_test == c)
      return 1;
    to_test++;
  }
  return 0;
}

int skip(char *buf, int i, char * to_skip) {
  while (is_in(buf[i], to_skip)) {
    i++;
  }
  return i;
}

static char json_start[] = " \t\r\n{";
static char key_start[] = ", \t\r\n\"";
static char keyval[]= "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789_-. *#";
static char keyvalsep[] = " \t\r\n\":";

void scpy(char* dest, char* src, int from, int to) {
  strncpy(dest, src+from, to-from);
  dest[to-from] = 0; 
}

int parse(char* buf, tsens_data *sens_data) {
  *sens_data = empty_sens_data;
  char keybuf[BUFLEN];
  char valuebuf[BUFLEN];
  int i = skip(buf, 0, json_start);
  int j;
  /* printf("buf: %s\n", buf); */
  do {
    i = skip(buf, i, key_start);
    j = skip(buf, i, keyval);
    /* printf("key %d-%d\n", i, j); */ 
    if (i == j) 
      break;
    scpy(keybuf, buf, i, j);
    i = skip(buf, j, keyvalsep);
    j = skip(buf, i, keyval);
    /* printf("value %d-%d\n", i, j); */ 
    /* empty value is ok */
    scpy(valuebuf, buf, i, j);
    i = skip(buf, j, keyvalsep);
    /* printf("%s : %s\n", keybuf,  valuebuf); */
    handle_key_value(keybuf, valuebuf, sens_data);
  } while (buf[i] != 0 && buf[i] != '}');
  return 0;
}

int wait_for_sock(int sock, int timeout_msecs) {  
  fd_set socket_read;
  memset(&socket_read, 0, sizeof(socket_read));
  FD_SET(sock, &socket_read);
  struct timeval tv;
  tv.tv_sec  = 0;
  tv.tv_usec = 0;
  if (timeout_msecs > 0) {
    tv.tv_sec  = timeout_msecs / 1000;
    tv.tv_usec = (timeout_msecs % 1000) * 1000;
  }
  if (select(sock+1, &socket_read, 0, 0, &tv) == -1) {
    return -1;
  } 
  return FD_ISSET(sock, &socket_read) != 0;
} 

static int receive_one_nonblocking(int i) {
  tsens_data sens_data;
  char buffer[BUFLEN];  
  int rec = 0;  
  int got;
  struct _con *con = cons + i;

  if (!con->is_open) 
    return -1;  
  got = 0;
  while (do_receive[i] && !got) {
    got = wait_for_sock(con->sock, 100);
    if (got < 0) {
      return -1; 
    }
    if (got == 0) {
      /* printf("timeouted\n"); */
      continue; /* next round */
    }
    /* now there MUST be something available */
    rec = recvfrom(con->sock, buffer, BUFLEN, 0, 
		   (struct sockaddr*) & con->echoclient, 
		   &con->clientlen);
    if (rec < 0) 
      return -2;
  }
  if (!do_receive[i]) { // we are done
    return 0;
  }
  parse(buffer, &sens_data);
  return con->callback(sens_data);
}


static int runargs[MAX_CON];
static void *receive_run(void *arg) {
  int i = *((int *) arg);
  while (do_receive[i]) {
    receive_one_nonblocking(i);
  }
  return 0;
}

static pthread_t receive_thread[MAX_CON];

int start_receiving(int port) {
  int i = find_con(port);
  if (i == -1)
    return -1;
  do_receive[i] = 1;
  runargs[i] = i;
  int ret = pthread_create(&receive_thread[i], NULL, receive_run, &runargs[i]);
  return ret;
}

int is_receiving(int port) {
  int i = find_con(port);
  if (i == -1)
    return 0;
  return do_receive[i] != 0;
}

int stop_receiving(int port) {
  int i = find_con(port);
  if (i == -1) 
    return 0; /* not even started */
  do_receive[i] = 0;
  /* will that work although do_receive is now an array? */
  pthread_join(receive_thread[i], NULL);
  return 0;
}

void get_open_ports(int ports[MAX_CON]) {
  int i;
  for (i=0; i < MAX_CON; i++) {
    ports[i] = 0;
    if ((cons[i].port != 0) && (do_receive[i] != 0)) {
      ports[i] = cons[i].port;
    }
  }
}

/* polling works only with one receiver 
 * shouldn't be used anyway */
static tsens_data poll_buffer;
static pthread_mutex_t poll_buffer_lock = PTHREAD_MUTEX_INITIALIZER;

int _save_to_poll_buffer(tsens_data data) {
  pthread_mutex_lock(&poll_buffer_lock);
  poll_buffer = data;
  pthread_mutex_unlock(&poll_buffer_lock);
  return 0;
}

int start_polling(int port) {
  int ret = sens_open(port, _save_to_poll_buffer);
  if (ret != 0)
    return ret;
  return start_receiving(port);
}

tsens_data sens_poll(int port) {
  int i = find_con(port);
  tsens_data ret = empty_sens_data;
  
  if (i == -1)
    return ret; /* empty */
  pthread_mutex_lock(&poll_buffer_lock);
  ret = poll_buffer;
  poll_buffer = empty_sens_data;
  pthread_mutex_unlock(&poll_buffer_lock);
  return ret;
}

int stop_polling(int port) {
  int ret = stop_receiving(port);
  if (ret != 0)
    return ret;
  return sens_close(port);
}



void show_sens_data(tsens_data sens_data) {
  printf("{x:%9.4f, y:%9.4f, z:%9.4f, k:%8x, timestamp:%10.2f, devid:%d}",
	 sens_data.x,
	 sens_data.y,
	 sens_data.z,
	 sens_data.keys,
	 sens_data.timestamp,
	 sens_data.devid);
}
